﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Text;
using IrrlichtLime.Core;
using IrrlichtLime.Scene;
using IrrKlang;
namespace BillardPHS4700 {


    class Simulation {
        Engine m_engine;
        public const int NUM_BALLS = 16;
        Vector3Df m_cueStickVelocity;
        bool m_cueStickHasHit;
        Vector3Df[] m_ballLinearVelocities = new Vector3Df[NUM_BALLS];
        Vector3Df[] m_ballAngularVelocitie = new Vector3Df[NUM_BALLS];
        int[] m_ballEstate = new int[NUM_BALLS];
        Plane3Df[] m_leftRightSides;
        Plane3Df[] m_topBottomSides;
        float[] m_leftRightSideLimits;
        float[] m_topBottomSideLimits;
        List<DiagonalWall> m_diagonalWalls;
        Line3Df[] m_collisionEdges;
        Vector3Df[] holes = new Vector3Df[6];
        int[] InHole = new int[NUM_BALLS];

        public Simulation(Engine engine) {
            m_engine = engine;
            for (int i = 0; i < m_ballLinearVelocities.Length; ++i) {
                m_ballLinearVelocities[i] = new Vector3Df(0f, 0f, 0f);
            }

            InitializeBorders();
        }


        void InitializeBorders() {
            var cornerWallLength = 5f;
            var middleWallLength = 2.5f;

            m_topBottomSides = new[] {
                new Plane3Df(0f, Constants.TopWallY, 0f, 0f, -1f, 0f),
                new Plane3Df(0f, Constants.BottomWallY, 0f, 0f, 1f, 0f)
            };
            m_leftRightSides = new[] {
                new Plane3Df(Constants.LeftWallX, 0f, 0f, 1, 0, 0),
                new Plane3Df(Constants.RightWallX, 0f, 0f, -1, 0, 0)
            };
            m_topBottomSideLimits = new[] { 52.5f, -52.5f  };
            m_leftRightSideLimits = new[] { 112.5f, 10f, -10f, -112.5f };
            
            // All diagonal walls can be represented by a point, a 2d vector and a normal
            m_diagonalWalls = new List<DiagonalWall> {
                // Top-left hole
                new DiagonalWall(m_leftRightSideLimits[0], Constants.TopWallY, cornerWallLength, cornerWallLength, 1f, -1f ),
                new DiagonalWall(Constants.LeftWallX, m_topBottomSideLimits[0],  cornerWallLength, cornerWallLength, -1f, 1f),
                // Top-right hole
                new DiagonalWall(m_leftRightSideLimits[3], Constants.TopWallY, -cornerWallLength, cornerWallLength, -1f, -1f),
                new DiagonalWall(Constants.RightWallX, m_topBottomSideLimits[0], -cornerWallLength, cornerWallLength, 1f, 1f),
                // Bottom-left hole
                new DiagonalWall(m_leftRightSideLimits[0], Constants.BottomWallY, cornerWallLength, -cornerWallLength, 1f, 1f ),
                new DiagonalWall(Constants.LeftWallX, m_topBottomSideLimits[1],  cornerWallLength, -cornerWallLength, -1f, -1f),
                // Bottom-right hole
                new DiagonalWall(m_leftRightSideLimits[3], Constants.BottomWallY, -cornerWallLength, -cornerWallLength, -1f, 1f ),
                new DiagonalWall(Constants.RightWallX, m_topBottomSideLimits[1],  -cornerWallLength, -cornerWallLength, 1f, -1f),
                // Middle-top hole
                new DiagonalWall(m_leftRightSideLimits[1], Constants.TopWallY, -cornerWallLength, middleWallLength, -1f, -1f),
                new DiagonalWall(m_leftRightSideLimits[2], Constants.TopWallY, cornerWallLength, middleWallLength, 1f, -1f),
                // Middle-bottom hole
                new DiagonalWall(m_leftRightSideLimits[1], Constants.BottomWallY, -cornerWallLength, -middleWallLength, -1f, 1f),
                new DiagonalWall(m_leftRightSideLimits[2], Constants.BottomWallY, cornerWallLength, -middleWallLength, 1f, 1f)
            };

            m_collisionEdges = m_diagonalWalls.Select(w => 
                new Line3Df(w.LimitA, new Vector3Df(w.LimitA.X, w.LimitA.Y, 1000f))).ToArray();

            //Initialisation of Holes positions
            //holes[0] = new Vector3Df(rightWallX - Constants.BallRadius * 0.707f, topWallY + Constants.BallRadius * 0.707f, Constants.TableHeight);
            float height = GetBallPosition(0).Z;
            holes[0] = new Vector3Df(Constants.RightWallX, Constants.TopWallY, height);
            holes[1] = new Vector3Df(0, Constants.TopWallY, height);
            holes[2] = new Vector3Df(Constants.LeftWallX, Constants.TopWallY, height);
            holes[3] = new Vector3Df(Constants.LeftWallX, Constants.BottomWallY, height);
            holes[4] = new Vector3Df(0, Constants.BottomWallY, height);
            holes[5] = new Vector3Df(Constants.RightWallX, Constants.BottomWallY, height);
            for (int i = 0; i < NUM_BALLS; ++i)
            {
                m_ballAngularVelocitie[i] = new Vector3Df(0, 0, 0);
                InHole[i] = 0;
                m_ballEstate[i] = 0;
            }
        }

        /// <summary>
        /// This parameter determines the speed of the simulation
        /// </summary>
        public float TimeStep { get; set; }

        /// <summary>
        /// When all balls have stopped, fire this event to let the gamestate know the shot is over
        /// </summary>
        public event EventHandler ShotFinished; 

        public void Shoot() {
            m_cueStickVelocity = m_engine.CueStick.Position * -4;
            m_cueStickVelocity.Z = 0; // cue stick always goes straight
        }

        /// <summary>
        /// Update the simulation by one timestep
        /// </summary>
        public void Update() {

            if (!m_cueStickHasHit) {
                m_engine.CueStick.Position += m_cueStickVelocity * TimeStep;
                CheckCueStickBallCollision();
            }
            else {
                Console.Write("boule blanche " + m_ballAngularVelocitie[0] + "\n");
                Console.Write("boule 1 " + m_ballAngularVelocitie[1] + "\n");
                Console.Write("boule 1 Rotation " + GetBallRotation(1) + "\n");
                // Apply linear velocities for all the balls
                for (int i = 0; i < NUM_BALLS; ++i) {
                    if (InHole[i] == 0)
                    {
                        UpdateBallCharacteristics(i);
                        /*
                        Vector3Df newVelocity;
                        if (m_ballLinearVelocities[i].Length == 0)
                        {
                            newVelocity = m_ballLinearVelocities[i];
                        }
                        else
                        {
                            newVelocity = m_ballLinearVelocities[i] - m_ballLinearVelocities[i] * ((TimeStep * 9.8f * Constants.LinearFriction) / (m_ballLinearVelocities[i].Length));
                        }

                        m_ballLinearVelocities[i] = newVelocity;
                        Vector3Df newRotation = GetBallRotation(i) + m_ballAngularVelocitie[i] * (TimeStep * 180f / 3.14f);
                        SetBallRotation(i, newRotation);
                         * */

                        //UpdateBallShadow(i);
                    }
                }
				
                CheckBallBallCollisions();

                CheckBallWallCollisions();

                CheckBallInHole();

                // Check if each ball has stopped; if so, end the simulation
                if (CheckBallsStopping()) {
                    EndSimulation();
                }
            }

            
        }

        void UpdateBallCharacteristics(int i){
            SetBallPosition(i, GetBallPosition(i) + m_ballLinearVelocities[i] * TimeStep);
            Vector3Df angularRectified = new Vector3Df(m_ballAngularVelocitie[i].X,- m_ballAngularVelocitie[i].Y,-m_ballAngularVelocitie[i].Z);
            SetBallRotation(i, GetBallRotation(i) - angularRectified * (TimeStep * 180f / 3.14f));
            

            Vector3Df pointToTable = new Vector3Df(0,0,-1f) * Constants.BallRadius;
            
            if(m_ballEstate[i] == 0){

                if (m_ballAngularVelocitie[i].Length != 0 && m_ballAngularVelocitie[i].Length != 0){
                    Vector3Df AngularMoment = m_ballAngularVelocitie[i]
                    * (-Constants.RotationFriction * Constants.BallRadius * Constants.BallWeight * 9.8f / (100 * Constants.InertieBall * m_ballAngularVelocitie[i].Length));
                    m_ballAngularVelocitie[i] = m_ballAngularVelocitie[i] + AngularMoment * TimeStep;

                    Vector3Df FormerVelocity = m_ballLinearVelocities[i];
                    m_ballLinearVelocities[i] = LocalCrossProd(m_ballAngularVelocitie[i], pointToTable);

                    if (m_ballLinearVelocities[i].DotProduct(FormerVelocity) < 0)
                    {
                        m_ballLinearVelocities[i] = new Vector3Df(0, 0, 0);
                        m_ballAngularVelocitie[i] = new Vector3Df(0, 0, 0);
                    }
                }
            }
            else{

                Vector3Df velocityPointOnTable = m_ballLinearVelocities[i] + LocalCrossProd(m_ballAngularVelocitie[i], pointToTable * (-1));
                float scal = velocityPointOnTable.DotProduct(m_ballLinearVelocities[i]);
                
                if(CheckChangeEstate(i,scal) == 0){
                    Vector3Df AngularMoment = LocalCrossProd(velocityPointOnTable, pointToTable) 
                        * (Constants.LinearFriction * 9.8f * Constants.BallWeight/(Constants.InertieBall * velocityPointOnTable.Length * 100));
                    m_ballAngularVelocitie[i] = m_ballAngularVelocitie[i] + AngularMoment * TimeStep;
                    
                    Vector3Df acceleration = velocityPointOnTable *
                        (- Constants.LinearFriction * 9.8f / velocityPointOnTable.Length);
                    m_ballLinearVelocities[i] = m_ballLinearVelocities[i] + acceleration * TimeStep;
                }
                else{
                    m_ballEstate[i] = 0;
                    m_ballLinearVelocities[i] = LocalCrossProd(m_ballAngularVelocitie[i], pointToTable);
                }
            }
        }

        int CheckChangeEstate(int i, float scal){
            if((m_ballEstate[i] == 1 && (scal <= 0)) || (m_ballEstate[i] == 2 && (scal >= 0))){
                return 1;
            }
            else{
                return 0;
            }
        }

        public static Vector3Df LocalCrossProd(Vector3Df w, Vector3Df x){
            Vector3Df Result = w.CrossProduct(x);
            Result.X = -Result.X;
            return Result;
        }

        void CheckCueStickBallCollision() {
            var distance = m_engine.CueStick.Position.Length;
            if (distance <= Constants.BallRadius) {
                // Point of collision in space is m_engine.CueStick.AbsolutePosition
                // Point of collision relative to ball center is m_engine.CueStick.Position
                // Don't forget the rotation of the cue is the rotation of its parent node, WhiteBallRoot
                Vector3Df whiteBallPosition = GetBallPosition(0);
                //m_ballLinearVelocities[0] = m_cueStickVelocity;
                //m_ballLinearVelocities[0].RotateXYby(m_engine.WhiteBallRoot.Rotation.Z);
                m_ballLinearVelocities[0] = Reaction.ReactionStickBallLinear(m_engine.CueStick.AbsolutePosition, m_cueStickVelocity, whiteBallPosition, m_engine.WhiteBallRoot);
                m_ballAngularVelocitie[0] = Reaction.ReactionStickBallAngular(m_engine.CueStick.AbsolutePosition, m_cueStickVelocity, whiteBallPosition);
                
                Vector3Df pointToTable = new Vector3Df(0, 0, -1f) * Constants.BallRadius;
                Vector3Df velocityPointOnTable = m_ballLinearVelocities[0] + LocalCrossProd(m_ballAngularVelocitie[0], pointToTable * (-1));
                float scal = velocityPointOnTable.DotProduct(m_ballLinearVelocities[0]);
                if (scal > 0){
                    m_ballEstate[0] = 1;
                }
                else if (scal < 0){
                    m_ballEstate[0] = 2;
                }
                else{
                    m_ballEstate[0] = 0;
                }
                
                // Stop and hide cue stick, record the hit
                m_cueStickVelocity = new Vector3Df(0f);
                m_cueStickHasHit = true;
                m_engine.CueStick.Visible = false;
            }
        }

        /// <summary>
        /// Check for collisions between balls
        /// </summary>
        void CheckBallBallCollisions() {
            for (int i = 0; i < NUM_BALLS; ++i) {
                var ball1Pos = GetBallPosition(i);
                for (int j = i + 1; j < NUM_BALLS; ++j) {
                    if (InHole[i] == 0 && InHole[j] == 0)
                    {
                        var ball2Pos = GetBallPosition(j);
                        var distance = (ball2Pos - ball1Pos).Length;
                        if (distance < 2 * Constants.BallRadius)
                        {
                            Vector3Df normal = ball1Pos - ball2Pos;
                            normal = normal * (1 / normal.Length);
                            SetBallPosition(i, GetBallPosition(j) + normal * 2 * Constants.BallRadius);
                            Vector3Df pointImpact = GetBallPosition(j) + normal * Constants.BallRadius;
                            // We have a ball-ball collision! Resolve reaction
                            // Simplistic method for now, each ball reverses its direction
                            //TODO implement real reaction
                            Reaction.ReactionBallBall(GetBallPosition(i), GetBallPosition(j), i, j, m_ballLinearVelocities, pointImpact);
                            m_ballAngularVelocitie[i] = new Vector3Df(0,0,0);
                            m_ballAngularVelocitie[j] = new Vector3Df(0,0,0);
                            m_ballEstate[i] = 1;
                            m_ballEstate[j] = 1;
                        }
                    }
                }
            }
        }

        void CheckBallInHole()
        {
            for (int i = 0; i < NUM_BALLS; ++i)
            {
                for (int h = 0; h < holes.Length; ++h)
                {
                    float distance = (holes[h] - GetBallPosition(i)).Length;
                    if (distance < Constants.BallRadius)
                    {
                        if (i == 0)
                        {
                            m_engine.WhiteBallRoot.Visible = false;
                            m_engine.Shadows[i].Visible = false;
                            m_ballLinearVelocities[i] = new Vector3Df(0,0,0);
                            SetBallPosition(i,new Vector3Df(130,0,Constants.TableHeight));
                            ISoundEngine soundEngine = new ISoundEngine();
                            soundEngine.Play2D("../assets/sounds/explosion.wav");
                        }
                        else
                        {
                            InHole[i] = 1;
                            m_engine.Balls[i-1].Visible = false;
                            m_engine.Shadows[i].Visible = false;
                            m_ballLinearVelocities[i] = new Vector3Df(0,0,0);
                            SetBallPosition(i,new Vector3Df(130,0,Constants.TableHeight));
                            ISoundEngine soundEngine = new ISoundEngine();
                            soundEngine.Play2D("../assets/sounds/bell.wav");
                        }
                    }
                }
            }
        }

        /// <summary>
        /// Check for collisions between balls and walls
        /// </summary>
        void CheckBallWallCollisions()
        {
            for (int i = 0; i < NUM_BALLS; ++i) {
                if (InHole[i] == 0)
                {
                    CheckWithSideWalls(i);
                    CheckWithTopBottomWalls(i);
                    CheckWithDiagonalWalls(i);
                    CheckWithEdges(i);
                }
            }
        }

        void CheckWithEdges(int numBall) {
            var ballPos = GetBallPosition(numBall);

            foreach (var edge in m_collisionEdges) {
                double distance;
                if (edge.GetIntersectionWithSphere(ballPos, Constants.BallRadius, out distance)) {
                    Reaction.ReactionBallWall(ballPos, numBall, m_ballLinearVelocities, edge.Vector);
                }
            }
        }

        /*
        void CheckWithSideWalls(int ballNum) {
            var ballPos = GetBallPosition(ballNum);
            // Check with side walls
            for (int j = 0; j < m_leftRightSides.Length; ++j) {
                var plane = m_leftRightSides[j];
                // If ball is colliding with this plane
                Vector3Df intersection;
                if (GetBallPlaneIntersection(ballPos, plane, out intersection)) {
                    // If the intersection is within the limits of this wall
                    if (intersection.Y < m_topBottomSideLimits[0] && intersection.Y > m_topBottomSideLimits[1]) {
                        // Then we have an actual collision
                        //TODO implement real reaction
                        float distance = Math.Abs(plane.GetDistanceTo(ballPos));
                        float delta = Constants.BallRadius - distance;
                        SetBallPosition(ballNum, ballPos - plane.Normal * delta);
                        Reaction.ReactionBallWall(GetBallPosition(ballNum), ballNum, m_ballLinearVelocities, intersection);
                    }
                }
            }
        }
         */

        void CheckWithSideWalls(int ballNum)
        {
            var ballPos = GetBallPosition(ballNum);
            for (int j = 0; j < m_leftRightSides.Length; ++j)
            {
                var plane = m_leftRightSides[j];
                float distance = Math.Abs(plane.GetDistanceTo(ballPos));
                if (distance < Constants.BallRadius)
                {
                    Vector3Df intersection = GetBallPosition(ballNum) - plane.Normal * Constants.BallRadius;
                    if (intersection.Y < m_topBottomSideLimits[0] && intersection.Y > m_topBottomSideLimits[1])
                    {
                        float delta = Constants.BallRadius - distance;
                        SetBallPosition(ballNum, ballPos - plane.Normal * delta);
                        Reaction.ReactionBallWall(GetBallPosition(ballNum), ballNum, m_ballLinearVelocities, intersection);
                        m_ballAngularVelocitie[ballNum] = new Vector3Df(0, 0, 0);
                        m_ballEstate[ballNum] = 1;
                    }
                }
            }
        }

        /*
        void CheckWithTopBottomWalls(int ballNum) {
            var ballPos = GetBallPosition(ballNum);
            for (int j = 0; j < m_topBottomSides.Length; ++j) {
                var plane = m_topBottomSides[j];
                // if ball is colliding with this plane
                Vector3Df intersection;
                if (GetBallPlaneIntersection(ballPos, plane, out intersection)) {
                    // If the intersection is within the limits of this wall ( multiple sections )
                    for (int k = 0; k < m_leftRightSideLimits.Length; k += 2) {
                        if (intersection.X < m_leftRightSideLimits[k] && intersection.X > m_leftRightSideLimits[k + 1]) {
                            // Then we have an actual collision
                            //TODO implement real reaction
                            float distance = Math.Abs(plane.GetDistanceTo(ballPos));
                            float delta = Constants.BallRadius - distance;
                            SetBallPosition(ballNum, ballPos - plane.Normal * delta);
                            Reaction.ReactionBallWall(GetBallPosition(ballNum), ballNum, m_ballLinearVelocities, intersection);
                        }
                    }
                }
            }
        }
         */

        void CheckWithTopBottomWalls(int ballNum)
        {
            var ballPos = GetBallPosition(ballNum);
            for (int j = 0; j < m_topBottomSides.Length; ++j)
            {
                var plane = m_topBottomSides[j];
                float distance = Math.Abs(plane.GetDistanceTo(ballPos));
                if (distance < Constants.BallRadius)
                {
                    Vector3Df intersection = GetBallPosition(ballNum) - plane.Normal * Constants.BallRadius;
                    for (int k = 0; k < m_leftRightSideLimits.Length; k += 2)
                    {
                        if (intersection.X < m_leftRightSideLimits[k] && intersection.X > m_leftRightSideLimits[k + 1])
                        {
                            float delta = Constants.BallRadius - distance;
                            SetBallPosition(ballNum, ballPos + plane.Normal * delta);
                            Reaction.ReactionBallWall(GetBallPosition(ballNum), ballNum, m_ballLinearVelocities, intersection);
                            m_ballAngularVelocitie[ballNum] = new Vector3Df(0, 0, 0);
                            m_ballEstate[ballNum] = 1;
                        }
                    }
                }
            }
        }

        void CheckWithDiagonalWalls(int ballNum) {
            var ballPos = GetBallPosition(ballNum);
            foreach (var diagWall in m_diagonalWalls) {
                Vector3Df intersection;
                if (GetBallPlaneIntersection(ballPos, diagWall.Plane, out intersection)) {
                    if (diagWall.IsBetweenLimits(intersection)) {
                        // Then we have collision with this wall
                        //TODO implement real reaction
                        Reaction.ReactionBallWall(GetBallPosition(ballNum), ballNum, m_ballLinearVelocities, intersection);
                        m_ballAngularVelocitie[ballNum] = new Vector3Df(0, 0, 0);
                        m_ballEstate[ballNum] = 1;
                    }
                }
            }
        }

        /*
        void CheckWithDiagonalWalls(int ballNum)
        {
            var ballPos = GetBallPosition(ballNum);
            foreach (var diagWall in m_diagonalWalls)
            {
                float distance = Math.Abs(diagWall.Plane.GetDistanceTo(ballPos));
                if (distance < Constants.BallRadius)
                {
                    float delta = Constants.BallRadius - distance;
                    SetBallPosition(ballNum, ballPos - diagWall.Plane.Normal * delta);
                    Vector3Df intersection = GetBallPosition(ballNum) - diagWall.Plane.Normal * Constants.BallRadius;
                    Reaction.ReactionBallWall(GetBallPosition(ballNum), ballNum, m_ballLinearVelocities, intersection);
                }
            }
        }
         */

        bool GetBallPlaneIntersection(Vector3Df ballPos, Plane3Df plane, out Vector3Df intersection) {
            intersection = null;
            if (Math.Abs(plane.GetDistanceTo(ballPos)) <= Constants.BallRadius) {
                return plane.GetIntersectionWithLine(ballPos, plane.Normal, out intersection);
            }
            return false;
        }

        /// <summary>
        /// Get the position of ball [num]
        /// </summary>
        /// <param name="num">0 for the white ball, 1-15 for the others</param>
        public Vector3Df GetBallPosition(int num) {
            return num == 0 ? 
                m_engine.WhiteBallRoot.Position : 
                m_engine.Balls[num - 1].Position;
        }

        /// <summary>
        /// Get the rotation of ball [num]
        /// </summary>
        /// <param name="num">0 for the white ball, 1-15 for the others</param>
        Vector3Df GetBallRotation(int num) {
            return num == 0 ? 
                m_engine.WhiteBallVisual.Rotation : 
                m_engine.Balls[num - 1].Rotation;
        }

        /// <summary>
        /// Sets the position of ball [num]
        /// </summary>
        /// <param name="num">0 for the white ball, 1-15 for the others</param>
        /// <param name="position">New position of the ball</param>
        void SetBallPosition(int num, Vector3Df position) {
            if (num == 0) {
                m_engine.WhiteBallRoot.Position = position;
            }
            else {
                m_engine.Balls[num - 1].Position = position;
            }
        }

        /// <summary>
        /// Sets the rotation of ball [num]
        /// </summary>
        /// <param name="num">0 for the white ball, 1-15 for the others</param>
        /// <param name="rotation">New rotation of the ball</param>
        void SetBallRotation(int num, Vector3Df rotation) {
            if (num == 0) {
                m_engine.WhiteBallVisual.Rotation = rotation;
            }
            else {
                m_engine.Balls[num - 1].Rotation = rotation;
            }
        }

        /// <summary>
        /// Stop balls that are too slow and returns whether all balls have stopped.
        /// </summary>
        bool CheckBallsStopping() {
            for (int i = 0; i < m_ballLinearVelocities.Length; ++i) {
                // if linear velocity is less than 1mm per second, stop ball
                if (m_ballLinearVelocities[i].LengthSQ < (0.1f * 0.1f)) {
                    m_ballLinearVelocities[i] = new Vector3Df(0);
                }
            }

            return m_ballLinearVelocities.All(v => v.LengthSQ <= 0.0001f);
        }

        /// <summary>
        /// Stops the simulation altogether, allowing the player to make his next shot.
        /// </summary>
        void EndSimulation() {
            m_cueStickHasHit = false;
            m_engine.ResetCueStick();
            m_engine.CueStick.Visible = true;
            if (ShotFinished != null) {
                ShotFinished(this, null);
            }
        }
    }
}
